/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
/**
 * @file NDS03_ContiunousRanging_Example.c
 * @author tongsheng.tang
 * @brief 连续测量示例
 * @version 2.x.x
 * @date 2024-04
 *
 * @copyright Copyright (c) 2024, Nephotonics Information Technology (Hefei) Co., Ltd.
 *
 */

#include <stdio.h>
#include "nds03_platform.h"
#include "nds03_comm.h"
#include "nds03_dev.h"
#include "nds03_data.h"
#include "nds03_calib.h"
#include "nds03_def.h"

#include "zino.h"
#include "drv_common.h"
#include <rtconfig.h>
#include <rtthread.h>

#define DBG_TAG "ND03/example"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

/** 模组设备 */
NDS03_Dev_t g_nds03_device = {
    .platform.i2c_dev_addr = NDS03_DEFAULT_SLAVE_ADDR,
    .platform.xshut_pin = GET_PIN(B, 6),
};

/**
 * @brief 单次测量接口
 *
 * @param pNxDevice 模组设备
 */
void SG_RangingTest(NDS03_Dev_t *pNxDevice)
{
    /* 获取测量数据 */
    NDS03_GetContinuousRangingData(pNxDevice);
    NX_PRINTF("depth1:%d mm, confi:%d, count:%d\r\n", pNxDevice->ranging_data[0].depth, pNxDevice->ranging_data[0].confi, pNxDevice->ranging_data[0].count);
    NX_PRINTF("depth2:%d mm, confi:%d, count:%d\r\n", pNxDevice->ranging_data[1].depth, pNxDevice->ranging_data[1].confi, pNxDevice->ranging_data[1].count);
    NX_PRINTF("depth3:%d mm, confi:%d, count:%d\r\n", pNxDevice->ranging_data[2].depth, pNxDevice->ranging_data[2].confi, pNxDevice->ranging_data[2].count);
    NX_PRINTF("depth4:%d mm, confi:%d, count:%d\r\n", pNxDevice->ranging_data[3].depth, pNxDevice->ranging_data[3].confi, pNxDevice->ranging_data[3].count);
}

/**
 * @brief NDS03连续测量主函数示例
 *
 * @return int32_t
 */
int32_t nds03_main(void)
{
    NDS03_Dev_t *pNxDevice = &g_nds03_device;
    /* 循环测距次数为100次 */
    int32_t cnt = 100;

    /* 初始化平台，包含I2C初始化 */
    if (NDS03_ERROR_NONE != nds03_platform_init(&pNxDevice->platform, NULL))
    {
        NX_PRINTF("nds03_platform init error\n");
        return NDS03_ERROR_API;
    }

    /* 初始化设备变量的IIC地址 */
    pNxDevice->platform.i2c_dev_addr = NDS03_DEFAULT_SLAVE_ADDR;

    /* 循环等待设备启动, 若模组或者IIC读写函数有问题则会报错 */
    if (NDS03_ERROR_NONE != NDS03_WaitDeviceBootUp(pNxDevice))
    {
        NX_PRINTF("NDS03_WaitDeviceBootUp error\r\n");
        return NDS03_ERROR_BOOT;
    }

    /* 初始化模组设备 */
    if (NDS03_ERROR_NONE != NDS03_InitDevice(pNxDevice))
    {
        NX_PRINTF("NDS03_InitDevice error!!\r\n");
        return NDS03_ERROR_INIT;
    }

    /* 连续模式开启测量 */
    NDS03_StartContinuousMeasurement(pNxDevice);

    /* 循环测量 */
    while (1)
    {
        SG_RangingTest(pNxDevice);
        // rt_thread_mdelay(10);
        // LOG_D("cnt = %d", cnt);
    }
    /* 连续模式关闭测量 */
    NDS03_StopContinuousMeasurement(pNxDevice);

    /* 释放平台 */
    nds03_platform_uninit(&pNxDevice->platform, NULL);

    return 0;
}
int nds03_continuous_ranging_init(NDS03_Dev_t *pNxDevice)
{
    /* 初始化平台，包含I2C初始化 */
    if (NDS03_ERROR_NONE != nds03_platform_init(&pNxDevice->platform, NULL))
    {
        NX_PRINTF("nds03_platform init error\n");
        return NDS03_ERROR_API;
    }

    /* 初始化设备变量的IIC地址 */
    pNxDevice->platform.i2c_dev_addr = NDS03_DEFAULT_SLAVE_ADDR;

    /* 循环等待设备启动, 若模组或者IIC读写函数有问题则会报错 */
    if (NDS03_ERROR_NONE != NDS03_WaitDeviceBootUp(pNxDevice))
    {
        NX_PRINTF("NDS03_WaitDeviceBootUp error\r\n");
        return NDS03_ERROR_BOOT;
    }

    /* 初始化模组设备 */
    if (NDS03_ERROR_NONE != NDS03_InitDevice(pNxDevice))
    {
        NX_PRINTF("NDS03_InitDevice error!!\r\n");
        return NDS03_ERROR_INIT;
    }

    /* 连续模式开启测量 */
    NDS03_StartContinuousMeasurement(pNxDevice);
    return 0;
}
int nds03_stop_measurement(NDS03_Dev_t *pNxDevice)
{
    /* 连续模式关闭测量 */
    NDS03_StopContinuousMeasurement(pNxDevice);

    /* 释放平台 */
    nds03_platform_uninit(&pNxDevice->platform, NULL);
    return 0;
}
int nds03_power_down(NDS03_Dev_t *pNxDevice)
{
    /* 关闭电源 */
    NDS03_SetXShutPinLevel(pNxDevice, 0);
    return 0;
}
int nd03_task_init()
{

    rt_thread_t nds03_thread = rt_thread_create("fc_error_handle", nds03_main, RT_NULL, 512, 10, 10);
    if (nds03_thread != RT_NULL)
    {
        rt_thread_startup(nds03_thread);
        LOG_I("nds03_thread create success.");
    }
    else
    {
        LOG_E("nds03_thread create failed.");
    }
    return 0;
}
ZINO_APP_EXPORT(nd03_task_init);

#include "sensor.h"
static rt_err_t nds03_control(struct rt_sensor_device *sensor, int cmd, void *args)
{
    rt_err_t result = RT_EOK;
    NDS03_Dev_t *pNxDevice = sensor->parent.user_data;
    switch (cmd)
    {
    case RT_SENSOR_CTRL_GET_ID:
        if (i2c_device_try("i2c2", NDS03_DEFAULT_SLAVE_ADDR) == 1)
        {
            *(rt_uint8_t *)args = NDS03_DEFAULT_SLAVE_ADDR;
        }
        break;
    case RT_SENSOR_CTRL_SET_POWER:
    {
        rt_uint32_t pctrl = (rt_uint32_t)args;
        if (pctrl == RT_SENSOR_POWER_DOWN)
        {
            LOG_D("NDS03 power down");
            nds03_power_down(pNxDevice);
        }
        else
        {
            LOG_D("NDS03 power up");
            nds03_continuous_ranging_init(pNxDevice);
        }
    }
    break;
    default:
        break;
    }
    return result;
}
static rt_size_t nds03_fect_data(struct rt_sensor_device *sensor, void *buf, rt_size_t len)
{
    struct rt_sensor_data *data = (struct rt_sensor_data *)buf;
    NDS03_Dev_t *pNxDevice = sensor->parent.user_data;
    if (sensor->info.type == RT_SENSOR_CLASS_TOF)
    {
        /* 获取测量数据 */
        NDS03_GetContinuousRangingData(pNxDevice);
        data->type = RT_SENSOR_CLASS_TOF;
        data->data.tof.confi = pNxDevice->ranging_data[0].confi;
        data->data.tof.count = pNxDevice->ranging_data[0].count;
        data->data.tof.crate = pNxDevice->ranging_data[0].crate;
        data->data.tof.depth = pNxDevice->ranging_data[0].depth;

    }
    return 1;
}

static struct rt_sensor_ops pmw3901mb_ops =
{
    nds03_fect_data,
    nds03_control,
};

int init_tof_sensor(void)
{
    rt_int8_t result;
    rt_sensor_t sensor = RT_NULL;
    static NDS03_Dev_t g_nds03_device = {
        .platform.i2c_dev_addr = NDS03_DEFAULT_SLAVE_ADDR,
        .platform.xshut_pin = GET_PIN(B, 6),
    };
    sensor->info.type = RT_SENSOR_CLASS_OP;
    sensor->info.vendor = RT_SENSOR_VENDOR_PIXART;
    sensor->info.model = "pmw3901mb";
    sensor->info.unit = RT_SENSOR_UNIT_PIXEL;
    sensor->info.intf_type = RT_SENSOR_INTF_SPI;
    sensor->info.range_max = 120;
    sensor->info.range_min = -120;
    sensor->info.period_min= 9;
    sensor->info.fifo_max = 0;

    sensor->ops = &pmw3901mb_ops;


    result = rt_hw_sensor_register(sensor, "pmw3901", RT_DEVICE_FLAG_RDWR, &g_nds03_device);

    return result;
}
